#include <chrono>
#include <thread>
#include <iostream>
#include "motor_drivers.hpp"
#include "motor_sockets/can_socket.hpp"


int main(){
    MotorDrivers::CANSocket DM4310_s1("can0", 0x41, 0x151, 8);
    MotorDrivers::MotorDM dm1(&DM4310_s1);
    dm1.motor_Init();

    MotorDrivers::CANSocket DM4310_s2("can0", 0x42, 0x152, 8);
    MotorDrivers::MotorDM dm2(&DM4310_s2);
    dm2.motor_Init();

    std::this_thread::sleep_for(std::chrono::milliseconds(1000));

    dm1.motor_Enable();
    dm1.motor_CleanPos();
    dm1.targetPos_ = 0.0;
    dm1.targetVel_ = 0.0;

    dm2.motor_Enable();
    dm2.motor_CleanPos();
    dm2.targetPos_ = 0.0;
    dm2.targetVel_ = 0.0;

    dm1.motor_Update();
    dm2.motor_Update();
    //cansend can0 151#0000

    while(true){
        int ch;
        std::cout<<"0/1、2/3" << std::endl;
        std::cin >> ch;
        switch (ch) {
            case 0:
                dm1.targetPos_ = 0.0;
                dm1.targetVel_ = 0.0;
                break;
            case 1:
                dm1.targetPos_ = 1.6;
                dm1.targetVel_ = 1.0;
                break;
            case 2:
                dm2.targetPos_ = 0.0;
                dm2.targetVel_ = 0.0;
                break;
            case 3:
                dm2.targetPos_  = 0.5;
                dm2.targetPos_ = 1.0;
                break;
            default: break;
        }
        dm1.motor_Update();
        dm2.motor_Update();
    }
}